/****************************** (C) COPYRIGHT 2016 Rsestudio ************************
* File Name          : 
* Author             : Rsestudio
* Version            : V1.0.0
* Date               : 01-MAR-2024
* Description        : 
*****************************************************************************************/

#ifndef __R_MATH_H__
#define __R_MATH_H__

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ----------------------------------------------------------------------------*/
/* Exported define ---------------------------------------------------------------------*/	
#define PID_OUTPUT_SCALER_MAX	        1000
#define PID_PONSTANT_DIFF_DEEP			16

#define MULTI_DPOINT_COUNT              8

/* Exported macro ----------------------------------------------------------------------*/
/* Exported typedef --------------------------------------------------------------------*/
typedef struct _pid{  
	int TargetPosition; 
	int ActualPosition;
	int err;
	int err_last;
	int err_Prev_1;
	int err_Prev_2;
	int err_Prev_3;
	int Diff_LastPolarity;
	int Integral_err;
	
	int IntegralLimit;
	int IntegralOverflow;
	int Process_Kp;
	int Process_Ki;
	int Process_Kd;
	
	int Constant_PID_DiffBuf[PID_PONSTANT_DIFF_DEEP];
	//Params
	//signed long Pid_Out;
}PID;


//******************************** filter ***********************************
typedef struct Crc32Data
{
    unsigned int  currentCrc;   //!< Current CRC value.
    unsigned int  byteCountCrc; //!< Number of bytes processed.
} crc32_data_t;

//******************************** filter ***********************************
typedef struct _kalman_filter
{
	double prevData; 
	double p;
	double q; 
	double r; 
	double kGain;
}kalman_filter_t;

typedef struct _average_filter
{
    int *p_avg_buf;
    int *p_avg_clc_buf;
    
    int avg_length;
    int avg_lose_cnt;
    
    int avg_point;
    
}average_filter_t;

//************************** point line ***********************************

typedef struct _dpoint_calib
{
	int 				raw_lower;
    int                 raw_higher;
    
	int 				real_lower;
    int                 real_higher; 

	int 				offset;
	
	float 				v_cal_k;
	float 				v_cal_b;
}dpoint_calib_t;

typedef struct _mdpoint_calib
{
    int                 calib_point;
	int 				offset;
	int 				p_x[MULTI_DPOINT_COUNT];
	int 				p_y[MULTI_DPOINT_COUNT];
		
	float 				v_cal_k[MULTI_DPOINT_COUNT];
	float 				v_cal_b[MULTI_DPOINT_COUNT];
}mdpoint_calib_t;

/* Exported variables ------------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------------------*/
void PID_init(PID *pPid);
int PIDIsA_Realize(PID *pPid,int Current);

//------------------------------------------------------------------------------------------------------

void crc32_init(crc32_data_t *crc32Config);
void crc32_update(crc32_data_t *crc32Config, const unsigned char *src, unsigned int lengthInBytes);
void crc32_finalize(crc32_data_t *crc32Config, unsigned int *hash);
unsigned int CRC32Software(unsigned char* pData, unsigned int Length);
unsigned short CRC16Software(unsigned char *pFrame,unsigned short Length);
unsigned char  CRC8Software(unsigned char *pFrame,unsigned short Length);
void EncryptBuffer(unsigned char* buffer, int size);
void DecryptBuffer(unsigned char* buffer, int size);
long abs_int(long x);

void kalmanFilter_init(kalman_filter_t *inData,float data,float q,float r);
float kalmanFilter_calc(kalman_filter_t *inData,float data);
int average_filter_init(average_filter_t *pft,int *pbuf_avg,int *pbuf_clc,int avg_length,int avg_lose,int data);
int average_filter_flush(average_filter_t *pft,int data);
int average_filter_add(average_filter_t *p_average,int x);
int average_filter_calc(average_filter_t *p_average,int* p_vmax,int* p_vmin);

void findPeaks(int *src, int src_lenth, short distance, short *indMax, short *indMax_len, short *indMin, short *indMin_len,short *buf_sign,char *buf_flag_max,char *buf_idelete,char *buf_temp_max);

void calib_dpoint_init(dpoint_calib_t *dpoint_cal);
int calib_dpoint_cal(dpoint_calib_t *dpoint_cal);
int dpoint_cal(dpoint_calib_t *dpoint,int raw);
int calib_mdpoint_cal(mdpoint_calib_t *mdpoint_cal);
int mdpoint_cal(mdpoint_calib_t *mdpoint,int raw);

signed short atan2s16(signed short y, signed short x);
int atan2s32(int y,int x) ;
unsigned int rs_sqrt(unsigned int num);

short high_pass_filter_s16(short input,int *p_prev,unsigned short alpha);
short low_pass_filter_s16(short input,int *p_prev,unsigned short alpha);

#ifdef __cplusplus
}
#endif

#endif
/**********************************(C)COPYRIGHT 2016 Rsestudio **************************/
/*****************************************END OF FILE************************************/

